/**
 ******************************************************************************
 * @FilePath: \Hero\Module\Chassis.c
 * @Brief: 
 * @Date: 2021-01-11 23:59:09
 * @Author: Rio
 ******************************************************************************
 */
#include "Chassis.h"
#include <string.h>
#include "bsp_usart.h"
#include "remoteCtrl.h"
#include "Motor_control.h"
#include "STMGood.h"
#include "Gimbal.h"
#include "pid.h"
#include "can.h"
#include <math.h>
#include <stdlib.h>
#include "bsp_can.h"
#include "Mode_SW.h"
#include "ramp.h"
#include "MonitorTask.h"
/*********************************** Private defines *****************************************/
#define ROTATE_SPD 3000
#define FACE_FORWARD_ENCODE     968
#define FACE_BACK_ENCODE        5026
#define RIGHT_SWING_ENCODE      1993
#define LEFT_SWING_ENCODE       8100
#define RAMP_INIT   100
#define ENCODE_ANGLE            0.0439506f //360/8191
#define POWER_LIMIT         60.0f
#define WARNING_POWER       30.0f
#define WARNING_POWER_BUFF  40.0f
#define BUFFER_TOTAL_CURRENT_LIMIT      16000.0f
#define POWER_TOTAL_CURRENT_LIMIT       20000.0f

/********************************** Private variables ****************************************/
int16_t velocity_y, velocity_x, yaw_rot;

chassisMove_t s_chassisMove = {0};
PID_AbsoluteType s_chassisMove_pos_pid = {0};
PID_AbsoluteType s_chassisMove_spd_pid = {0};
PID_AbsoluteType s_chassisSwing_pos_pid = {0};
PID_AbsoluteType s_chassisSwing_spd_pid = {0};

motorData_t s_chassis_motor[4] = {0};
PID_AbsoluteType s_chassis_motor_pid[4] = {0};
PID_AbsoluteType s_chassis_motorLim_pid[4] = {0};

int16_t spd[4];
float	motor[4];
float energy;
float ratio[4];
extern uint8_t capExpect_txBuff[8];
/********************************** Extern variables ****************************************/
extern remoteCtrl_t s_rcData;
extern motorData_t s_gimbal_yaw_motor;
extern PID_AbsoluteType s_gimbal_yaw_motor_pos_pid;
extern float p,P,i,I,d,D;
extern wl2data chassis_c;
extern wl2data chassis_v;
extern data1_4 chassis_power;
extern roboSetting_t roboSet;
extern OFFLINE_COUNT_STRUCT  calFps;
/**************************** Static function declaration ************************************/
static float loop_float_constrain(float Input, float minValue, float maxValue);
/********************************** Static function ****************************************/
static float ChasFollow_yawrot(int16_t target, PID_AbsoluteType *s_chassisMove_pos_pid, PID_AbsoluteType *s_chassisMove_spd_pid)
{
	float yawRot;

	s_chassisMove_pos_pid->errNow = target - s_gimbal_yaw_motor.esc_back_position;

	if(s_chassisMove_pos_pid->errNow > 4095)
		s_chassisMove_pos_pid->errNow -= 8191;
	else if(s_chassisMove_pos_pid->errNow < -4096)
		s_chassisMove_pos_pid->errNow += 8191;
	s_chassisMove_pos_pid->errNow *= ENCODE_ANGLE;

  PID_AbsoluteMode(s_chassisMove_pos_pid);
	s_chassisMove_spd_pid->errNow = s_chassisMove_pos_pid->ctrOut - s_gimbal_yaw_motor.esc_back_speed;
	PID_AbsoluteMode(s_chassisMove_spd_pid);
	yawRot = s_chassisMove_spd_pid->ctrOut;

	return yawRot;
}
/**
  * @brief          limit the power, mainly limit motor current
  * @param[in]      chassis_power_control: chassis data
  * @retval         none
  */
static void chassis_power_control(chassisMove_t *s_chassisMove, float motorr[4])
{
	s_chassisMove->cur_sum_limit = 30000;
	if(s_chassisMove->chassis_power_buffer < 20.0f)
		s_chassisMove->cur_sum_limit = 16 * s_chassisMove->chassis_power_buffer * s_chassisMove->chassis_power_buffer;
	if(s_chassisMove->capVol < 17.0f)
		s_chassisMove->cur_sum_limit = 15 * s_chassisMove->capVol * s_chassisMove->capVol;
	else s_chassisMove->cur_sum_limit = 30000;
	if(calFps.judge > 8)
	{
		if(s_chassisMove->capVol > 17.0f)
		{
			capExpect_txBuff[0] = (uint16_t)(s_chassisMove->chassis_power_limit*100+s_chassisMove->chassis_power_buffer*70) >> 8;
			capExpect_txBuff[1] = (uint16_t)(s_chassisMove->chassis_power_limit*100+s_chassisMove->chassis_power_buffer*70);
			memset(&capExpect_txBuff[2], 0, 6);
		}
		else
		{
			capExpect_txBuff[0] = (uint16_t)(s_chassisMove->chassis_power_limit*100) >> 8;
			capExpect_txBuff[1] = (uint16_t)(s_chassisMove->chassis_power_limit*100);
			memset(&capExpect_txBuff[2], 0, 6);
		}
	}
	else
	{
		capExpect_txBuff[0] = (uint16_t)(60*100) >> 8;
		capExpect_txBuff[1] = (uint16_t)(60*100);
		memset(&capExpect_txBuff[2], 0, 6);
	}

	if(s_chassisMove->cur_sum > s_chassisMove->cur_sum_limit)
	{
		float scale = (float)s_chassisMove->cur_sum_limit / (float)s_chassisMove->cur_sum;
		for(int8_t i=0;i<4;i++)
		{
			motorr[i] *= scale;
		}
	}
}

//循环限幅函数
static float loop_float_constrain(float Input, float minValue, float maxValue)
{
    if (maxValue < minValue)
    {
        return Input;
    }

    if (Input > maxValue)
    {
        float len = maxValue - minValue;
        while (Input > maxValue)
        {
            Input -= len;
        }
    }
    else if (Input < minValue)
    {
        float len = maxValue - minValue;
        while (Input < minValue)
        {
            Input += len;
        }
    }
    return Input;
}
/********************************** Interface function ****************************************/
void ChassisMotor_Init(void)
{
	for(int i=0;i<4;i++) 
	memset(&s_chassis_motor[i], 0, sizeof(motorData_t));//对电机信息结构体进行清零初始化
	memset(&s_chassisMove     , 0, sizeof(chassisMove_t));

	PID_AbsParam_Init(&s_chassisMove_pos_pid, 6, 0, 0, 0, 800);
	PID_AbsParam_Init(&s_chassisMove_spd_pid, 30, 0, 1, 2000, 28000);
	PID_AbsParam_Init(&s_chassisSwing_pos_pid, 4, 0, 0, 0, 800);
	PID_AbsParam_Init(&s_chassisSwing_spd_pid, 30, 0, 1, 2000, 28000);

	PID_AbsParam_Init(&s_chassis_motor_pid[0], 12, \
																						3, \
																						0, \
																						4000, \
																						10000);
	PID_AbsParam_Init(&s_chassis_motor_pid[1], 8, \
																						0, \
																						0, \
																						0, \
																						10000);
	PID_AbsParam_Init(&s_chassis_motor_pid[2], \
																						10, \
																						0, \
																						0, \
																						4000, \
																						10000);
	PID_AbsParam_Init(&s_chassis_motor_pid[3], 10, \
																						0, \
																						0, \
																						4000, \
																						10000);
}

void Chassis_Move(void)
{
	static int8_t direct = 0;
  s_chassisMove.cur_sum = 0;
//	PID_AbsParam_Set(&s_chassis_motor_pid[0], p, i, d, 4000, 10000);
	// PID_AbsParam_Init(&s_chassis_motor_pid[3], P, I, D, 4000, 10000);

	velocity_y = (s_rcData.rc.ch[1]-1024)*10000/(1684-1024);
	velocity_x = (s_rcData.rc.ch[0]-1024)*10000/(1684-1024);

	if((s_rcData.key.v & KEY_PRESSED_OFFSET_SHIFT))
	{
		if(s_rcData.key.v & CHASSIS_FRONT_KEY) velocity_y =  10000;
		if(s_rcData.key.v & CHASSIS_BACK_KEY)  velocity_y = -10000;
		if(s_rcData.key.v & CHASSIS_LEFT_KEY)  velocity_x = -10000;
		if(s_rcData.key.v & CHASSIS_RIGHT_KEY) velocity_x =  10000;
	}
	else
	{
		if (s_rcData.key.v & CHASSIS_FRONT_KEY)
		{
			velocity_y = 6000 * ramp_cal(&ramp_w);
			ramp_w.interval = 0;
			ramp_s.count = RAMP_INIT;
		}
		else
		{
			ramp_w.interval++;
			if(ramp_w.interval>3) ramp_w.count = RAMP_INIT;
		}

		if (s_rcData.key.v & CHASSIS_BACK_KEY)
		{
			velocity_y = -6000 * ramp_cal(&ramp_s);
			ramp_s.interval = 0;
			ramp_w.count = RAMP_INIT;
		}
		else
		{
			ramp_s.interval++;
			if(ramp_s.interval>3) ramp_s.count = RAMP_INIT;
		}

		if (s_rcData.key.v & CHASSIS_LEFT_KEY)
		{
				velocity_x = -6000 * ramp_cal(&ramp_a);
				ramp_a.interval = 0;
				ramp_d.count = RAMP_INIT;
		}
		else
		{
			ramp_a.interval++;
			if(ramp_a.interval>3) ramp_a.count = RAMP_INIT;
		}

		if (s_rcData.key.v & CHASSIS_RIGHT_KEY)
		{
				velocity_x = 6000 * ramp_cal(&ramp_d);
				ramp_d.interval = 0;
				ramp_a.count = RAMP_INIT;
		}
		else
		{
			ramp_d.interval++;
			if(ramp_d.interval>3) ramp_d.count = RAMP_INIT;
		}
	}

	s_chassisMove.angle_diff = (float)(s_gimbal_yaw_motor.serial_position-FACE_FORWARD_ENCODE)/8191*360.0f;

	float angDiff = loop_float_constrain(s_chassisMove.angle_diff, -180.0f, 180.0f)*(3.1415926/180);
	float sinA = sinf(angDiff);
	float cosA = cosf(angDiff);

	s_chassisMove.Vx = cosA*velocity_x+sinA*velocity_y;
	s_chassisMove.Vy = cosA*velocity_y-sinA*velocity_x;
	if(s_chassisMove.capVol > 20.0f)
		s_chassisMove.Wz = ROTATE_SPD+(s_chassisMove.capVol-20.0f)/(23.0f-20.0f)*(6000-ROTATE_SPD);
	else if(s_chassisMove.capVol > 18.0f)
		s_chassisMove.Wz = ROTATE_SPD+(s_chassisMove.capVol-18.0f)/(20.0f-18.0f)*(4000-ROTATE_SPD);

	switch(roboSet.e_chassisMode)
	{
		case C_FOLLOW:
		{
			yaw_rot = -ChasFollow_yawrot(FACE_FORWARD_ENCODE, &s_chassisMove_pos_pid, &s_chassisMove_spd_pid);
			spd[0] =  (velocity_x-velocity_y+yaw_rot) * ramp_cal(&ramp_c);
			spd[1] =  (velocity_x+velocity_y+yaw_rot) * ramp_cal(&ramp_c);
			spd[2] = (-velocity_x+velocity_y+yaw_rot) * ramp_cal(&ramp_c);
			spd[3] = (-velocity_x-velocity_y+yaw_rot) * ramp_cal(&ramp_c);
      for(int8_t i=0;i<4;i++)
			{
				motor[i] = motor_speed_loop_pid_control(&s_chassis_motor_pid[i], s_chassis_motor[i].esc_back_speed, spd[i]);
				s_chassisMove.cur_sum += fabs(motor[i]);
			}
			break;
		} 
		case C_FOLLOW_BACK:
		{
			yaw_rot = -ChasFollow_yawrot(FACE_BACK_ENCODE, &s_chassisMove_pos_pid, &s_chassisMove_spd_pid);
			spd[0] =  (s_chassisMove.Vx-s_chassisMove.Vy+yaw_rot) * ramp_cal(&ramp_c);
			spd[1] =  (s_chassisMove.Vx+s_chassisMove.Vy+yaw_rot) * ramp_cal(&ramp_c);
			spd[2] = (-s_chassisMove.Vx+s_chassisMove.Vy+yaw_rot) * ramp_cal(&ramp_c);
			spd[3] = (-s_chassisMove.Vx-s_chassisMove.Vy+yaw_rot) * ramp_cal(&ramp_c);
      for(int8_t i=0;i<4;i++)
			{
				motor[i] = motor_speed_loop_pid_control(&s_chassis_motor_pid[i], s_chassis_motor[i].esc_back_speed, spd[i]);
				s_chassisMove.cur_sum += fabs(motor[i]);
			}
			break;
		} 
		case C_ROTATE:
		{
			ramp_c.count = 60;
			spd[0] = 0.5*( s_chassisMove.Vx-s_chassisMove.Vy)+s_chassisMove.Wz;
			spd[1] = 0.5*( s_chassisMove.Vx+s_chassisMove.Vy)+s_chassisMove.Wz;
			spd[2] = 0.5*(-s_chassisMove.Vx+s_chassisMove.Vy)+s_chassisMove.Wz;
			spd[3] = 0.5*(-s_chassisMove.Vx-s_chassisMove.Vy)+s_chassisMove.Wz;
			for(int8_t i=0;i<4;i++)
			{
				motor[i] = motor_speed_loop_pid_control(&s_chassis_motor_pid[i], s_chassis_motor[i].esc_back_speed, spd[i]);
				s_chassisMove.cur_sum += fabs(motor[i]);
			}
			break;
		}	
		case C_SWING:
		{
			if(direct==0)
				{
					yaw_rot = -ChasFollow_yawrot(LEFT_SWING_ENCODE, &s_chassisSwing_pos_pid, &s_chassisSwing_spd_pid);
					if(fabs(s_chassisSwing_pos_pid.errNow)<5.0f) direct = 1;
				}
			else
				{
					yaw_rot = -ChasFollow_yawrot(RIGHT_SWING_ENCODE, &s_chassisSwing_pos_pid, &s_chassisSwing_spd_pid);
					if(fabs(s_chassisSwing_pos_pid.errNow)<5.0f) direct = 0;
				}

			spd[0] =  (s_chassisMove.Vx-s_chassisMove.Vy+yaw_rot);
			spd[1] =  (s_chassisMove.Vx+s_chassisMove.Vy+yaw_rot);
			spd[2] = (-s_chassisMove.Vx+s_chassisMove.Vy+yaw_rot);
			spd[3] = (-s_chassisMove.Vx-s_chassisMove.Vy+yaw_rot);

      for(int8_t i=0;i<4;i++)
			{
				motor[i] = motor_speed_loop_pid_control(&s_chassis_motor_pid[i], s_chassis_motor[i].esc_back_speed, spd[i]);
				s_chassisMove.cur_sum += fabs(motor[i]);
			}
			break;
		} 
		case C_CLAW:
		{
			for(int8_t i=0;i<4;i++)
			{
				motor[i] = motor_speed_loop_pid_control(&s_chassis_motor_pid[i], s_chassis_motor[i].esc_back_speed, 0);
				s_chassisMove.cur_sum += fabs(motor[i]);
			}
			break;
		} 
		case C_NULL:
		{
			for(int8_t i=0;i<4;i++) motor[i] = 0;
			break;
		}	
	}

	chassis_power_control(&s_chassisMove, motor);
	//电机输出

	CAN_Send_bytes(&hcan1,0x210,capExpect_txBuff);
	CANTx_SendCurrent(&hcan1, 0x200, motor[0], motor[1], motor[2], motor[3]);
}
